#pragma once

#include "siyi_protocol.hpp"

#include <cassert>
#include <cmath>
#include <mutex>
#include <deque>
#include <thread>
#include <iostream>
#include <optional>
#include <vector>
// #include "log/log.h"

namespace siyi {

    class Camera {
    public:
        Camera() {
            thread_receive = std::thread(&Camera::receive_thread, this);
            thread_process = std::thread(&Camera::process_thread, this);
            m_stop = false;
        }

        ~Camera() {
            send_temperature_request(0);
            if (thread_receive.joinable()) {
                thread_receive.join();
            }
            if (thread_process.joinable()) {
                thread_process.join();
            }
        }

        void stop(){
            m_stop = true;
        }

        void connect_camera(const std::string &ip, int port) {
            // init

            if (!_Messenger.setup(ip, port)) {
                // LOG_ERROR("Failed to setup messenger.");
                return;
            }

            _Messenger.send(_serializer.assemble_message(siyi::GetAttitude{}), "GetAttitude");
            send_temperature_request(2);
        }

        bool init() {
            if (ack_attitude.has_value()) {
                // LOG_INFO("Camera initialized successfully.");
                return true;
            } else {
                // LOG_ERROR("Camera initialization failed.");
                return false;
            }
        }

        std::pair<double, double> get_current_attitude() {
            AckGetAttitude attitude = ack_attitude.value();

            double curr_yaw, curr_pitch;
            // yaw
            curr_yaw = double(attitude.yaw) / 10.0;
            // pitch
            double pitch = double(attitude.pitch) / 10.0;
            if (90.0 <= pitch && pitch <= 180) curr_pitch = 180.0 - pitch;
            if (-180.0 <= pitch && pitch <= -155) curr_pitch = 180.0 + pitch;

            return std::make_pair(curr_yaw, curr_pitch);
        }

        void controlCamera(double &curr_yaw, double &curr_pitch) {
            siyi::SetGimbalAttitude set_gimbal_attitude{};
            // LOG_INFO("Set yaw :{}, pitch: {} ", curr_yaw, curr_pitch);
            set_gimbal_attitude.pitch_t10 = static_cast<std::int16_t>(curr_pitch * 10);
            set_gimbal_attitude.yaw_t10 = static_cast<std::int16_t>(curr_yaw * 10);
            mtx.lock();
            _Messenger.send(_serializer.assemble_message(set_gimbal_attitude), "SetGimbalAttitude");
            mtx.unlock();
        }

        void send_temperature_request(int get_temp_flag) {
            auto get_temperature = siyi::GetTemperature{};
            get_temperature.get_temp_flag = get_temp_flag;
            mtx.lock();
            _Messenger.send(_serializer.assemble_message(get_temperature), "GetTemperature");
            mtx.unlock();
        }

        std::vector<int> get_current_temperature() {
            if (!ack_temperature.has_value()) return {};
            std::vector<int> temp{ack_temperature.value().temp_max,
                                  ack_temperature.value().temp_max_x,
                                  ack_temperature.value().temp_max_y};
            ack_temperature.reset();
            return temp;
        }

        void receive_thread() {
            while (true) {
                if(m_stop)
                    break;
                std::vector<std::uint8_t> udp_message;
                std::uint8_t cmd_id;
                _Messenger.receive(udp_message, cmd_id);
                if (udp_message.empty())
                    continue;

                mtx.lock();
                udp_message_deque.push_back(std::make_pair(cmd_id, udp_message));
                mtx.unlock();
            }
        }

        void process_thread() {
            std::cout<<"process_thread start" << std::endl;
            while (true) {
                if(m_stop)
                    break;
                std::this_thread::sleep_for(std::chrono::milliseconds(10)); // 避免忙等
                mtx.lock();
                if (udp_message_deque.empty()) {
                    //std::cout << "udp_message_deque" << std::endl;
                     mtx.unlock();
                    continue;
                }

                auto msg = udp_message_deque.front();
                auto cmd_id = msg.first;
                auto udp_message = msg.second;
                std::cout << "Processing thread is running. Cmd ID: 0x" << std::hex << std::setw(2) << std::setfill('0')
                          << static_cast<int>(cmd_id) << std::endl;

                switch (cmd_id) {
                    case 0x01:
                        ack_version = _deserializer.disassemble_message<siyi::AckFirmwareVersion>(udp_message);
                        // LOG_INFO(" AckFirmwareVersion Received\n ");
                        break;
                    case 0x0D:
                        ack_attitude = _deserializer.disassemble_message<siyi::AckGetAttitude>(udp_message);
                        // LOG_INFO("AckGetAttitude Received\n");
                        break;
                    case 0x14:
                        ack_temperature = _deserializer.disassemble_message<siyi::AckGetTemperature>(udp_message);
                        // LOG_INFO("AckGetTemperature Received");
                        break;
                    default:
                        std::cout << "[WARNING] Unknown command ID: 0x" << std::hex << std::setw(2) << std::setfill('0')
                                  << static_cast<int>(cmd_id) << std::endl;
                        break;
                }

                udp_message_deque.pop_front();
                mtx.unlock();
            }
        }

    private:
        Serializer _serializer;
        Deserializer _deserializer;
        Messenger _Messenger;

        std::optional<AckFirmwareVersion> ack_version;
        std::optional<AckGetAttitude> ack_attitude;
        std::optional<AckGetTemperature> ack_temperature;

        std::mutex mtx;

        std::thread thread_receive, thread_process;

        std::deque<std::pair<int, std::vector<std::uint8_t>>> udp_message_deque;
        bool m_stop;
    };

} // siyi
